%%%%%%% kitti %%%%%%% 
% 2011_10_03 & 2011_9_30 & 2011_9_26
Rvi=[9.999976e-01 7.553071e-04 -2.035826e-03 -7.854027e-04 9.998898e-01 -1.482298e-02 2.024406e-03 1.482454e-02 9.998881e-01];
Rvi=reshape(Rvi,3,3)';
tvi=[-8.086759e-01 3.195559e-01 -7.997231e-01]';
Tvi=[Rvi tvi;0 0 0 1];

% 2011_9_26
% Rcv=[7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02];
% Rcv=reshape(Rcv,3,3)';
% tcv=[-4.069766e-03 -7.631618e-02 -2.717806e-01]';
% Tcv=[Rcv tcv;0 0 0 1];

% 2011_9_30
% Rcv=[7.027555e-03 -9.999753e-01 2.599616e-05 -2.254837e-03 -4.184312e-05 -9.999975e-01 9.999728e-01 7.027479e-03 -2.255075e-03];
% Rcv=reshape(Rcv,3,3)';
% tcv=[-7.137748e-03 -7.482656e-02 -3.336324e-01]';
% Tcv=[Rcv tcv;0 0 0 1];

% 2011_10_03
% Rcv=[7.967514e-03 -16.99968e-01 -8.462264e-04 -2.771053e-03 8.241710e-04 -9.999958e-01 9.999644e-01 7.969825e-03 -2.764397e-03];
% Rcv=reshape(Rcv,3,3)';
% tcv=[-1.377769e-02 -5.542117e-02 -2.918589e-01]';
% Tcv=[Rcv tcv;0 0 0 1];


% Tci = Tcv * Tvi;
% Tic = inv(Tci)


%%%%%%%  urban %%%%%%% 
Rvc=[-0.00680499 -0.0153215 0.99985 -0.999977 0.000334627 -0.00680066 -0.000230383 -0.999883 -0.0153234];
Rvc=reshape(Rvc,3,3)';
tvc=[1.64239 0.247401 1.58411]';
Tvc=[Rvc tvc;0 0 0 1];

Rvi=[1 0 0 0 1 0 0 0 1];
Rvi=reshape(Rvi,3,3)';
tvi=[-0.07 0 1.7]';
Tvi=[Rvi tvi;0 0 0 1];
% Tic = inv(Tvi) * Tvc
Tic = Tvi \ Tvc

Rc2c=stereoParams.RotationOfCamera2';
tc2c=stereoParams.TranslationOfCamera2'/1000;
Tc2c=[Rc2c tc2c;0 0 0 1]
% Tic2 = Tic* inv(Tc2c)
Tic2 = Tic /Tc2c


